/*
 * @Author: your name
 * @Date: 2021-05-01 19:36:53
 * @LastEditTime: 2021-05-06 18:55:19
 * @LastEditors: Please set LastEditors
 * @Description: In User Settings Edit
 * @FilePath: \gcxz\Middlewares\Device\chassis.h
 */
#ifndef _CHASSIS_H
#define _CHASSIS_H

#include "motor.h"
#include "standard.h"
#include "MK60_ftm.h"
#include "data_process.h"

class Chassis{
public:
    Chassis(){}
    Vector3 target_vel;                // 目标速度（x,y,z分量）
    Vector3 target_pose;               // 目标姿态（x,y,z分量）
    Vector3 current_pose;              // 当前姿态（x,y,z分量）
    myPID pose_pid[3];                 // x,y,z方向上的pid（3个）

    motor motors[4] = {
        motor(ftm3, ftm_ch0, ftm_ch1), // LF
        motor(ftm3, ftm_ch3, ftm_ch2), // RF
        motor(ftm3, ftm_ch7, ftm_ch6), // RB
        motor(ftm3, ftm_ch4, ftm_ch5), // LB
    };
    void init();                       // 初始化数据，务必调用
    void setTargetVel(Vector3 vel);    // 设置x,y,z三轴速度
    void setTargetPose(Vector3 pose);  // 设置目标位姿
    void updateCurrentPose(Vector3 pose);
    void control();                    // 解算得到各个电机目标值，并控制电机
    void adjust();                     // 微调整函数
    void setMotorPid(float _Kp, float _Ki, float _Kd, // 通过底盘给电机设置pid参数（用在速度环），内部调用电机的设置pid
		float _I_Term_Max, float _Out_Max);
	void setMotorZero();                // 设置电机PWM为0
	
	void testMotorForward();            // 电机pwm =  6000向前运动
	void testMotorBackward();           // 电机pwm = -6000向后运动
	void testMotorMax();                // 电机pwm =  9500最大速度运动
    void testPinYi(int i);                    // 测试平移的
    void setMotorPwm(int i);
private:
	// TODO: 开环测试，暂时赋值为9500
    float max_vel_x = 9500; // cm/s
    float max_vel_y = 9500; // cm/s
    float max_vel_z = 9500; // cm/s

    void resolve();                     // 解算电机速度
    void controlPose();                 // 控制底盘位姿
    void adjustPose();                  // 控制底盘微调整姿态
};  


#endif
